#include "common.h"

Matrix3x3::Matrix3x3(double matrix[3][3]) : data{
    {matrix[0][0], matrix[0][1], matrix[0][2]},
    {matrix[1][0], matrix[1][1], matrix[1][2]},
    {matrix[2][0], matrix[2][1], matrix[2][2]}
} {}


Vector3d Matrix3x3::multiply(const Vector3d &v) const
{
    return {
        data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z,
        data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z,
        data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z};
}

Matrix3x3 Matrix3x3::transpose() const {
    double matrix[3][3] = {
        {data[0][0], data[1][0], data[2][0]},
        {data[0][1], data[1][1], data[2][1]},
        {data[0][2], data[1][2], data[2][2]}
    };
    return Matrix3x3(matrix);
}

AllData::AllData()
{
    memset(this, 0, sizeof(AllData)); // 清零内存
}

// 时间戳更新函数组
void AllData::updateT1()
{
    t1 = std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::system_clock::now().time_since_epoch())
             .count();
}

void AllData::updateT2()
{
    t2 = std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::system_clock::now().time_since_epoch())
             .count();
}

void AllData::updateT3()
{
    t3 = std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::system_clock::now().time_since_epoch())
             .count();
}

// 获取当前时间戳（微秒级）
uint64_t AllData::getCurrentTimestamp()
{
    return std::chrono::duration_cast<std::chrono::microseconds>(
               std::chrono::high_resolution_clock::now().time_since_epoch())
        .count();
}

// 判断两个时间戳是否相差0.5秒内
bool AllData::isOk(uint64_t ts1, uint64_t ts2, uint64_t threshold_)
{
    return std::abs(static_cast<int64_t>(ts1 - ts2)) <= threshold_;
}